function [ dDalphaBl, DalphaTildeSl ] = gyroerror_inc( DalphaBl, gyroPar, Tl, DupsilonBl, ignore2Od ) %#codegen
%GYROERROR_INC 计算陀螺器件误差引入的测量误差
%
% Tips
% # 本函数针对增量形式
%
% Input Arguments
% # DalphaBl: 3*n矩阵，n为采样个数，角增量，单位rad
% # Tl: 标量，采样周期，单位s
% # gyroPar: 各域为数组的结构体，陀螺的确定性误差参数，各域最后一维为时间m
%        gyroPar.KScal0: 3*n向量，陀螺标度因数标称值，单位：1rad对应的脉冲数
%        gyroPar.PS0B：3*3*n矩阵，传感器系与本体系之间的坐标转换矩阵标称值，无单位，默认全为单位阵
%        gyroPar.domegaIBBiasS: 3*n向量，零偏，单位：rad/s
%        gyroPar.dKScalP: 3*x*n数组，x为考虑的标度因数误差的阶次，x=1，模型为线性，陀螺正向标度因数误差，单位与阶次相关，第1列无单位，第i列单位(rad/s)^-(i-1)，默认全为0
%        gyroPar.dKScalN: 3*x*n数组，x为考虑的标度因数误差的阶次，x=1，模型为线性，陀螺正向标度因数误差，单位与阶次相关，第1列无单位，第i列单位(rad/s)^-(i-1)，默认全为0
%        gyroPar.dPSS0：3*3*n矩阵，失准角（安装误差），无单位，默认全为0
%        gyroPar.domegaIBQuantS: 3*n向量，量化误差，单位：rad/s，默认全为0
%        gyroPar.DGSens: 3*3*n矩阵，g敏感项系数，单位：(rad/s)/(m/s^2)，默认全为0
% # DupsilonBl: 3*n矩阵，比速度增量，单位m/s
% # ignore2Od: 逻辑标量，true表示忽略2阶误差，默认为true
%
% Output Arguments
% # dDalphaBl: 3*n矩阵，角增量误差，单位rad
% # DalphaTildeSl: 3*n矩阵，各采样时刻S系下的角增量（包含测量误差），单位rad
%
% References:
% # 《应用导航算法工程基础》“惯性传感器模型” 总误差模型

if nargin < 5
    ignore2Od = true;
end
n = size(DalphaBl, 2);

dDalphaBl = coder.nullcopy(NaN(3, n));
if nargout > 1
    DalphaTildeSl = coder.nullcopy(NaN(3, n));
end
for j=1:n
    %% 计算陀螺仪各项误差
    % 陀螺g敏感项
    domegaIBGSensSInt = gyroPar.DGSens(:, :, j)*DupsilonBl(:, j);
    %% 计算陀螺角增量输出值dalphaTildeS与角增量理论值dalpha之差
    % 不加标度因数误差阵的角增量输出
    domegaBiasExSInt = gyroPar.domegaIBBiasS(:, j)*Tl + domegaIBGSensSInt + gyroPar.domegaIBQuantS(:, j)*Tl;
    DalphaSl = (gyroPar.PS0B(:, :, j)*(eye(3)+gyroPar.dPSS0(:, :, j)))'*DalphaBl(:, j);
    DalphaTildeScalSl = DalphaSl + gyroPar.domegaIBBiasS(:, j)*Tl + domegaIBGSensSInt;
    % 计算j时刻标度因数误差阵
    gyrodKScalCoef = scalfacterrsign(gyroPar.dKScalP(:, :, j), gyroPar.dKScalN(:, :, j), DalphaTildeScalSl);
    % 计算dKScal
    dKScal = polyscalfact(DalphaTildeScalSl, gyrodKScalCoef, ones(3, 1), Tl); % NOTE: vTildeScalS计算式应与fgjacobian及imuout2in函数一致
    % 忽略二阶误差的陀螺误差
    dPSBT = (gyroPar.PS0B(:, :, j)*gyroPar.dPSS0(:, :, j))';
    dDalphaBl(:, j) = (gyroPar.PS0B(:, :, j)')\(dKScal.*(gyroPar.PS0B(:, :, j)'*DalphaBl(:, j))+dPSBT*DalphaBl(:, j)+domegaBiasExSInt);
    if ~ignore2Od
        % 考虑二阶误差的加速度计误差
        dDalphaBl2 = (gyroPar.PS0B(:, :, j)')\(dKScal.*(dPSBT*DalphaBl(:, j)+domegaBiasExSInt));
        dDalphaBl(:,j) = dDalphaBl(:,j)+dDalphaBl2;
    end
    if nargout > 1
        DalphaTildeSl(:, j) = (ones(3, 1)+dKScal) .* (DalphaSl+domegaBiasExSInt);
    end
end
end